/**
 * @file    jm_frame_port.c
 * @brief   关节电机数据帧接口服务
 * @details This is the detail description.
 * @author  wang-haoyu
 * @date    2025年7月10日 (created)
 * @version 0.0.1
 * @par Copyright (C):
 *          All Rights Reserved.
 * @par Encoding:
 *          UTF-8
 * @par Description        :
 * 1. Hardware Descriptions:
 *      None.
 * 2. Program Architecture:
 *      None.
 * 3. File Usage:
 *      None.
 * 4. Limitations:
 *      None.
 * 5. Else:
 *      None.
 * @par Modification:
 * -# Date          : 2025年7月10日;
 * Revision         : 0.0.1;
 * Author           : wang-haoyu;
 * Contents         :
 */
 
/* =============================================================================
 *                                header files
 * ========================================================================== */
#include "jm_frame_port.h"

/* =============================================================================
 *                              macros & constants
 * ========================================================================== */

/* =============================================================================
 *                          data structure definitions
 * ========================================================================== */
static jm_drv_frame_t jm_drv_frame = JM_FRAME_INITALIZER;
/* =============================================================================
 *                         static variable definitions
 * ========================================================================== */

/* =============================================================================
 *                         static function definitions
 * ========================================================================== */

/* =============================================================================
 *                         global function definitions
 * ========================================================================== */

/*
 * 关节电机数据帧解包函数
 *
 * Parameter:
 *   Input:
 *		-*fifo_src：fifo接口句柄
 *      -*analys_src:关节电机数据解析参数
 *   Output:
 *      -*info_obj：关节电机数据信息
 * Returns:
 *		-JM_DRV_ENOERR_CODE：返回状态
 */
JM_DRV_ENOERR_CODE jm_drv_frame_unpack(jm_fifo_port_t *fifo_src,jm_data_analys_param_t *analys_src,jm_drv_info_get_t *info_obj,utils_timer_t *timer)
{
    struct rt_can_msg unpack_msg = {0};
    rt_int32_t fifo_cnt = 0;
    int pos = 0,vel = 0,tor = 0;
    fifo_cnt = jm_drv_fifo_get_used(fifo_src);
    if(0 > fifo_cnt) 
        return E_JM_DRV_DATA_LENERR;

    jm_drv_fifo_read(fifo_src,&unpack_msg,sizeof(unpack_msg));

	pos = (unpack_msg.data[1]<<8)|unpack_msg.data[2];
	vel = (unpack_msg.data[3]<<4)|(unpack_msg.data[4]>>4);
	tor = ((unpack_msg.data[4] & 0x0f)<<8)|unpack_msg.data[5];

    info_obj[unpack_msg.id - 1].motor_id = unpack_msg.id;
    info_obj[unpack_msg.id - 1].motor_sta_word = (unpack_msg.data[0])>>4;
	info_obj[unpack_msg.id - 1].motor_mos_tempreture = (float)unpack_msg.data[6];
	info_obj[unpack_msg.id - 1].motor_rotor_tempreture = (float)unpack_msg.data[7];
	
	info_obj[unpack_msg.id - 1].motor_pos = uint_to_float(pos, analys_src[unpack_msg.id - 1].jm_pos_min, analys_src[unpack_msg.id - 1].jm_pos_max, 16);
	info_obj[unpack_msg.id - 1].motor_vel = uint_to_float(vel, analys_src[unpack_msg.id - 1].jm_vel_min, analys_src[unpack_msg.id - 1].jm_vel_max, 12);
	info_obj[unpack_msg.id - 1].motor_torque = uint_to_float(tor, analys_src[unpack_msg.id - 1].jm_tor_min,analys_src[unpack_msg.id - 1].jm_tor_max, 12);

	info_obj[unpack_msg.id - 1].connect_sta = true;

	utils_timer_countdown_ms(&timer[unpack_msg.id - 1],1000);
}

/*
 * 关节电机错误码分析函数
 *
 * Parameter:
 *   Input:
 *		-src:输入
 * Returns:
 * 		错误码
 */
rt_uint32_t jm_drv_error_code_analys(rt_uint8_t src)
{
	return jm_drv_frame.jm_drv_err_code_analys(src);
}

/*
 * 关节电机使/失能设置函数
 *
 * Parameter:
 *   Input:
 *		-node_id:驱动器ID号
 *		-sta_set：设置状态
 * Returns:
 */
void jm_drv_enable_set_frame(rt_uint16_t node_id,bool sta_set)
{
	jm_drv_frame.jm_drv_enable_set_state(node_id,sta_set);
}

/*
 * 关节电机使/失能紧急设置函数
 *
 * Parameter:
 *   Input:
 *		-node_id:驱动器ID号
 *		-sta_set：设置状态
 * Returns:
 */
void jm_drv_enable_set_urgent_frame(rt_uint16_t node_id,bool sta_set)
{
	jm_drv_frame.jm_drv_enable_set_urgent_state(node_id,sta_set);
}

/*
 * 关节电机零点设置函数
 *
 * Parameter:
 *   Input:
 *		-node_id:驱动器ID号
 * Returns:
 */
void jm_drv_zero_point_set_frame(rt_uint16_t node_id)
{
	jm_drv_frame.jm_drv_zero_point_set_state(node_id);
}

/*
 * 关节电机清错设置函数
 *
 * Parameter:
 *   Input:
 *		-node_id:驱动器ID号
 * Returns:
 */
void jm_drv_clear_error_frame(rt_uint16_t node_id)
{
	jm_drv_frame.jm_drv_clear_error_state(node_id);
}

/*
 * 关节电机控制函数
 *
 * Parameter:
 *   Input:
 *		-node_id:驱动器ID号
 *		-pos_des:位置设置
 *		-vel_des:速度限制设置
 *		-i_des:力矩设置
 * Returns:
 */
void jm_drv_ctrl_frame(rt_uint16_t node_id,float pos_des,rt_uint16_t vel_des,rt_uint16_t i_des)
{
	jm_drv_frame.jm_drv_ctrl_state(node_id,pos_des,vel_des,i_des);
}

/*******************************************************************************
 *                                  EOF
 ******************************************************************************/